#include "MCIncidentConstraint.h"
#include <iostream>
#include <MCIncidentConstraintVisual.h>

using namespace cg;

MCIncidentContraint::MCIncidentContraint(IGripPoint *grip1, IGripPoint *grip2) :
    _grip1(grip1),
    _grip2(grip2),
    _rootPoint(grip2->position()),
    _presenter(grip1->canvas()),
    _label(new MCIncidentConstraintVisual(_presenter, this))
{
    _solver = CreateSolver();
    _presenter->addVisualItem(_label);
}

MCIncidentContraint::~MCIncidentContraint()
{
    RemoveSolver(_solver);
}

void MCIncidentContraint::resolve()
{
    Point p1 = CreatePoint(_solver, _grip1->position().x, _grip1->position().y);
    Point p2 = CreatePoint(_solver, _grip2->position().x, _grip2->position().y);

    if (_grip1->position() != _rootPoint && _grip2->position() == _rootPoint)
    {
        _rootPoint = _grip1->position();
        Point root = CreatePoint(_solver, _rootPoint.x, _rootPoint.y);
        Cnstr c = makeIncident(p1, root);

        if (Solve(_solver))
        {
            _grip2->setPositionAt(MCPoint2D(VariableValue(_solver, p1.x), VariableValue(_solver, p1.y)));
        }

        RemoveConstraint(_solver, c);
        RemovePoint(_solver, root);
    }
    else if (_grip2->position() != _rootPoint && _grip1->position() == _rootPoint)
    {
        _rootPoint = _grip2->position();
        Point root = CreatePoint(_solver, _rootPoint.x, _rootPoint.y);
        Cnstr c = makeIncident(p2, root);

        if (Solve(_solver))
        {
            _grip1->setPositionAt(MCPoint2D(VariableValue(_solver, p2.x), VariableValue(_solver, p2.y)));
        }

        RemoveConstraint(_solver, c);
        RemovePoint(_solver, root);
    }

    RemovePoint(_solver, p1);
    RemovePoint(_solver, p2);
}

Cnstr MCIncidentContraint::makeIncident(cg::Point &p1, cg::Point &p2)
{
    return Cnstr {
        AddEquation(_solver, p1.x - p2.x),
        AddEquation(_solver, p1.y - p2.y)

    };
}
